#pragma once

#include "interface/controller.h"

#include "path_follow_controller.h"

#include <cmath>
#include <thread>

class StanleyController : public PathFollowController {
public:
    StanleyController() {};

    void update(const std::vector<Odometry> &poses) override { pose_ = poses[1]; };

    /** PathFollowController **/
    float CalcSteeringAngle(int lookahead_idx) override;

    float CalcLinearVel(float steer_angle) override;

    void FollowThread() override;

private:
    // 角度归一化到[-π, π]
    double normalizeAngle(double angle) const {
        while (angle > M_PI) angle -= 2.0 * M_PI;
        while (angle < -M_PI) angle += 2.0 * M_PI;
        return angle;
    }

    const float k_        = 2.0;  // Stanley控制器系数
    const float softener_ = 0.2;  // 松软因子
};
